#include <pantheios/pantheios.hpp>
#include <pantheios/inserters.hpp>
#include <math.h>
#include <cassert>
#include "player_car.h"
#include "serverinterfaces.h"
#include "ltservershell.h"
#include "server_physics.h"
#include "msgids.h"
#include "racing_logic_shell.h"





namespace GameLogic
{


    LTVector player_car::do_position() const
    {
        LTVector posistion;
        g_pLTServer->GetObjectPos(m_clientObj, &posistion);
        return posistion ;
    }
   
    LTRotation player_car::do_rotation() const {
        LTRotation rotation;
        g_pLTServer->GetObjectRotation(m_clientObj, &rotation);
        return rotation ;
    }

  

    void player_car::do_wheel_pos_rot(int ids, LTVector& pos,
                                      LTRotation& rot) {
        assert(ids < 4) ;
        server_physics* physics = m_logic_shell->physics_system() ;
        hkVector4 pp ;
        hkQuaternion qq ;
        physics->car_wheel_pos_rot(id(), ids, pp, qq) ;
        LTVector np(pp(0), pp(1), pp(2)) ;
        pos = m_logic_shell->physics_position_transfer_to_game_position(np) ;
        LTRotation nr(qq(0), qq(1), qq(2),qq(3)) ;
        rot = m_logic_shell->physics_rotation_transfer_to_game_rotation(nr) ;
    }


    void player_car::do_update_action_logic() {
    }


    void player_car::do_update_car_steer(float timeInteval)
    {
        if(is_car_try_to_speed_up_forward((car_action_mask)current_action()) ||
           is_car_try_to_speed_up_backward((car_action_mask)current_action()))
        {
            if(is_car_try_to_speed_up_forward((car_action_mask)current_action()))
            {
                y_steer() -= 0.01f * timeInteval;
            }
            else
            {
                y_steer() += 0.01f * timeInteval;
            }
            y_steer() = hkMath::clamp(y_steer(), -1.0f, 1.0f);
        }
        else
        {
            y_steer() = 0.0f;
        }
        server_physics* physics = m_logic_shell->physics_system();
        // FIXME
        physics->steer_car_action(
            current_action(),
            steer_x_current(),
            steer_y_current(),
            physics->find_car_by_id(id()),
            y_steer(),
            timeInteval) ;


    }


    void player_car::do_update_pos_rotation()
    {
        server_physics* physics = m_logic_shell->physics_system();
        hkVector4 pos = physics->car_position(id());
        const hkQuaternion& rot = physics->car_rotation(id());
        // FIXME for test only
        LTVector jpos(pos(0), pos(1), pos(2));

        LTRotation jrot(rot(0),rot(1),rot(2), rot(3));
        
       jpos -= 0.5f * jrot.Up();
        jpos = m_logic_shell->physics_position_transfer_to_game_position(jpos);
	


        
        jrot = m_logic_shell->physics_rotation_transfer_to_game_rotation(jrot);

        set_speed(physics->car_speed(id()));
        
        LTVector up = jrot.Up();
        

        
          LTVector ppos ;
        g_pLTServer->GetObjectPos(m_clientObj, &ppos) ;
        //g_pLTServer->CPrint("bef pos is %f %f %f", ppos.x, ppos.y, ppos.z) ;
      
        
        //jpos -= 0.8 * up ;
       // g_pLTServer->CPrint("pos dir is %f %f %f", jpos.x, jpos.y, jpos.z) ;

        g_pLTServer->SetObjectPos(m_clientObj, &jpos);
      
        g_pLTServer->GetObjectPos(m_clientObj, &ppos) ;
       // g_pLTServer->CPrint("get pos is %f %f %f", ppos.x, ppos.y, ppos.z) ;
        
        g_pLTServer->SetObjectRotation(m_clientObj, &jrot);
        update_wheels() ;


    }

 
    player_car::player_car(int id, HOBJECT clientObj,
                           HCLIENT clientId, 
                           racing_logic_shell* shell)
        :car(id),
         m_logic_shell(shell),
         m_clientObj(clientObj),
         m_clientId(clientId) {

    }

    player_car::~player_car() {
        if(m_clientObj) {
            g_pLTServer->RemoveObject(m_clientObj);
            m_clientObj = 0;
        }        
    }


}
